#ifndef __FIVE_POINT_GRADIENT_DESCENT__
#define __FIVE_POINT_GRADIENT_DESCENT__

#include <iostream>
#include <geometry_msgs/Pose.h>
using namespace std;

// 五点中值梯度下降
// https://blog.csdn.net/weixin_43879302/article/details/116092810
// https://blog.csdn.net/qq_35635374/article/details/138291015

class FivePointGradientDescent{
    public:
        FivePointGradientDescent(){};
        ~FivePointGradientDescent() = default;

        void pathSmooth(vector<geometry_msgs::Pose> poses_in, vector<geometry_msgs::Pose>& poses_out,
            std::function<bool(double x, double y)> f){
            int iterations = 50;

            poses_out = poses_in;
            int n = poses_in.size();
            if (n < 5) {
                // 如果路径点少于5个，无法进行五点平滑，直接返回
                return;
            }
        
            // P = -1/6 * (P-2 - 4P-1 + 6P - 4P1 + P2)
            float a = -1.0 / 6.0;
        
            // 进行多次迭代
            for (int iter = 0; iter < iterations; ++iter) {
                // 对路径中的每个点（从第2个到倒数第2个）
                for (int i = 2; i < n - 2; ++i) {
                    geometry_msgs::Pose prev_2_pose = poses_out[i - 2];
                    geometry_msgs::Pose prev_1_pose = poses_out[i - 1];
                    geometry_msgs::Pose curr_pose = poses_out[i];
                    geometry_msgs::Pose back_1_pose = poses_out[i + 1];
                    geometry_msgs::Pose back_2_pose = poses_out[i + 2];
        
                    // 计算梯度
                    geometry_msgs::Pose cache_point = curr_pose;
                    cache_point.position.x += a * (prev_2_pose.position.x 
                                                    - 4.0 * prev_1_pose.position.x 
                                                    + 6.0 * curr_pose.position.x 
                                                    - 4.0 * back_1_pose.position.x 
                                                    + back_2_pose.position.x);
                    cache_point.position.y += a * (prev_2_pose.position.y 
                                                    - 4.0 * prev_1_pose.position.y 
                                                    + 6.0 * curr_pose.position.y 
                                                    - 4.0 * back_1_pose.position.y 
                                                    + back_2_pose.position.y);
                    // 判断是否存在障碍物
                    if(f(cache_point.position.x, cache_point.position.y)) {
                        // 更新当前点位置
                        poses_out[i] = cache_point;
                    }    
                }
            }            
        };      
};


#endif